import sensor
import image
import time

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)

red = [(22, 80, 35, 77, -50, 22)]
green = [(60, 88, -56, -29, -26, 7)]
blue = [(47, 71, -27, 4, -71, -31)]

while(True):
    img = sensor.snapshot()

    for color, color_name, color_rgb in zip([red, green, blue], ["Red", "Green", "Blue"], [(255, 0, 0), (0, 255, 0), (0, 0, 255)]):
        blob = img.find_blobs(color, area_threshold=1000, margin=10)
        if blob:
            for b in blob:
                img.draw_cross(b[5], b[6], color=color_rgb)
                img.draw_edges(b.min_corners(), color=color_rgb)
                print("{} block center at: ({}, {})".format(color_name, b[5], b[6]))

